#include "align_enu/align_enu_wrapper.h"
#include "pcl_conversions/pcl_conversions.h"
#include "pcl/common/transforms.h"
align_ENU_Wrapper::align_ENU_Wrapper(/* args */)
{
    ros::NodeHandle nh;
    nh.param<std::string>("lidar_odom_topic",lidar_odom_topic,"/cloud_lio");
    nh.param<std::string>("gnss_imu_odom_topic",gnss_imu_odom_topic,"/gia/odom");
    nh.param<bool>("visualization",visualization,false);
    nh.param<int>("opt_times",opt_times,1);
    nh.param<float>("opt_distance",opt_distance,20.f);
    nh.param<int>("opt_frame_nums",opt_frame_nums,40);
    nh.param<std::vector<double>>("mapping/extrinsic_T", extrinT, std::vector<double>());
    nh.param<std::vector<double>>("mapping/extrinsic_R", extrinR, std::vector<double>());
    Eigen::Matrix3d rot;
    rot<<extrinR[0],extrinR[1],extrinR[2],extrinR[3],extrinR[4],extrinR[5],extrinR[6],extrinR[7],extrinR[8];
    extrin_q  = rot;
    extrin_t<<extrinT[0],extrinT[1],extrinT[2];
    lidar_odom_sub = nh.subscribe(lidar_odom_topic,100,&align_ENU_Wrapper::lidar_odom_sub_cb,this);
    gnss_imu_odom_sub = nh.subscribe(gnss_imu_odom_topic,100,&align_ENU_Wrapper::gnss_imu_odom_sub_cb,this);
    init_q_sub = nh.subscribe("imu_init_q",100,&align_ENU_Wrapper::init_imu_q_sub_cb,this);
    // optimization_srv = nh.advertiseService("opt",&align_ENU_Wrapper::optimization_srv_cb,this);
    cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("visual_cloud",100);
    lidar_path_pub = nh.advertise<nav_msgs::Path>("visual_lidar_path",100);
    gnss_path_pub = nh.advertise<nav_msgs::Path>("visual_gnss_path",100);
    trans_by_enu_path = nh.advertise<nav_msgs::Path>("visual_enu_path",100);
    trans_by_imu_path = nh.advertise<nav_msgs::Path>("visual_imu_path",100);
    cloud_enu_pub = nh.advertise<cloud_pose_msg::CloudPose>("cloud_enu",100);
    
    // save_path_srv = nh.advertiseService("save_path",&align_ENU_Wrapper::save_path_cb,this);
    gnss_path.header.frame_id = "map";
    q.setIdentity();
    t.setZero();
    run();
}
align_ENU_Wrapper::~align_ENU_Wrapper()
{
}
void align_ENU_Wrapper::init_imu_q_sub_cb(const geometry_msgs::Quaternion&msg){
    if(!init_q_vailed){

        init_q_vailed = true;
        init_imu_q.x() = msg.x;
        init_imu_q.y() = msg.y;
        init_imu_q.z() = msg.z;
        init_imu_q.w() = msg.w;
        init_q_sub.shutdown();
    }
}
void align_ENU_Wrapper::lidar_odom_sub_cb(const cloud_pose_msg::CloudPoseConstPtr&msg){
    cloud_pose_msg::CloudPose cpm = *msg;
    auto &lio_position = cpm.pose.position;
    auto &lio_qua = cpm.pose.orientation;
    Eigen::Vector3d position = {lio_position.x,lio_position.y,lio_position.z};
    lidar_origin_path.push_back(position);

    ae.add_origin_path_point(position,msg->header.stamp.toSec());
    Eigen::Quaterniond rot(lio_qua.w,lio_qua.x,lio_qua.y,lio_qua.z);
    rot = q*rot;
    rot.normalize();
    position = q*position+t+rot*extrin_t;
    rot = rot*extrin_q;
    // . 更新到ENU坐标系下的位姿
    lio_position.x = position.x();
    lio_position.y = position.y();
    lio_position.z = position.z();
    lio_qua.w = rot.w();
    lio_qua.x = rot.x();
    lio_qua.y = rot.y();
    lio_qua.z = rot.z();
    cpm.status =  converge;
    cloud_enu_pub.publish(cpm);
    if(visualization)pub_visualization_msg(cpm);
}
void align_ENU_Wrapper::gnss_imu_odom_sub_cb(const nav_msgs::Odometry::ConstPtr&msg){
    static int cnt = 0;
    Eigen::Vector3d position = {msg->pose.pose.position.x,msg->pose.pose.position.y,0};
    if(gnss.size()<2){
        gnss.resize(2,position);
    }
    gnss[1] = position;
    if(cnt%2==0)ae.add_align_to_path_point(position,msg->header.stamp.toSec());
    cnt++;
    if(visualization){
        geometry_msgs::PoseStamped psd;
        psd.pose = msg->pose.pose;
        gnss_path.poses.push_back(psd);
        gnss_path_pub.publish(gnss_path);
    }

}
void align_ENU_Wrapper::run(){
    ros::Rate rate(100);
    int opt_cnt = 0;
    while(ros::ok()){
        if(ae.get_align_to_point_size()>opt_frame_nums&&gnss.size()==2&&(gnss[0]-gnss[1]).norm()>opt_distance){
            double score;
            converge = ae.optimization(score);
            ae.get_result(q,t);
            opt_cnt++;
            if(converge&&opt_times>0&&opt_cnt>=opt_times){
                std::cout<<"complete!--------------------------------------------------------------------"<<std::endl;
                ros::spin();
            }
        }
        rate.sleep();
        ros::spinOnce();
    }
}
// bool align_ENU_Wrapper::optimization_srv_cb(std_srvs::Trigger::Request&req,std_srvs::TriggerResponse&res){
//     double score;
//     // res.success =  ae.optimization(score);
//     // res.message = "score: "+std::to_string(score);
//     // ae.get_result(q,t);
//     nav_msgs::Path path,path1;
//     for (auto point : lidar_origin_path)
//     {
//         point = q*point+t;
//         geometry_msgs::PoseStamped psd;
//         psd.pose.position.x = point.x();
//         psd.pose.position.y = point.y();
//         psd.pose.position.z = point.z();
//         path.poses.push_back(psd);
        
//     }
//     for (auto point : lidar_origin_path)
//     {
//         point = imu_q*point;
//         geometry_msgs::PoseStamped psd;
//         psd.pose.position.x = point.x();
//         psd.pose.position.y = point.y();
//         psd.pose.position.z = point.z();
//         path1.poses.push_back(psd);
        
//     }
//     path.header.frame_id = "map";
//     path1.header.frame_id = "map";
//     all_path.publish(path);
//     trans_by_imu.publish(path1);
//     return true;
// }
void align_ENU_Wrapper::pub_visualization_msg(const cloud_pose_msg::CloudPose &msg){

    
    nav_msgs::Path path1;
    nav_msgs::Path lidar_path;
    for (auto point : lidar_origin_path)
        {
            geometry_msgs::PoseStamped psd;
            psd.pose.position.x = point.x();
            psd.pose.position.y = point.y();
            psd.pose.position.z = point.z();
            lidar_path.poses.push_back(psd);
            
    }
    lidar_path.header.frame_id = "map";
    lidar_path_pub.publish(lidar_path);
    if(init_q_vailed){
        for (auto point : lidar_origin_path)
        {
            point = init_imu_q*point;
            geometry_msgs::PoseStamped psd;
            psd.pose.position.x = point.x();
            psd.pose.position.y = point.y();
            psd.pose.position.z = point.z();
            path1.poses.push_back(psd);
            
        }   
        path1.header.frame_id = "map";
        trans_by_imu_path.publish(path1);
    }
    if(converge){
        nav_msgs::Path path;
        for (auto point : lidar_origin_path)
        {
            point = q*point+t;
            geometry_msgs::PoseStamped psd;
            psd.pose.position.x = point.x();
            psd.pose.position.y = point.y();
            psd.pose.position.z = point.z();
            path.poses.push_back(psd);
        }
        path.header.frame_id = "map";
        trans_by_enu_path.publish(path);
    }
    pcl::PointCloud<pcl::PointXYZI> cloud;
    pcl::fromROSMsg(msg.cloud,cloud);
    Eigen::Quaternion<float>qf;
    qf.x() = msg.pose.orientation.x;
    qf.y() = msg.pose.orientation.y;
    qf.z() = msg.pose.orientation.z;
    qf.w() = msg.pose.orientation.w;
    Eigen::Matrix4f trans;
    trans.setIdentity();
    trans.block<3,3>(0,0) = qf.toRotationMatrix();
    trans(0,3) = msg.pose.position.x;
    trans(1,3) = msg.pose.position.y;
    trans(2,3) = msg.pose.position.z;
    pcl::transformPointCloud(cloud,cloud,trans);
    sensor_msgs::PointCloud2 ros_cloud;
    pcl::toROSMsg(cloud,ros_cloud);
    ros_cloud.header.frame_id = "map";
    cloud_pub.publish(ros_cloud);
}
// bool align_ENU_Wrapper::save_path_cb(std_srvs::Trigger::Request&r,std_srvs::Trigger::Response &re){
//     re.success = true;
//     re.message = "save";
//     double score;
//     //converge = ae.optimization(score);
//     //ae.get_result(q,t);
//     std::ofstream lidar_file;
//     std::ofstream gnss_file;
//     lidar_file.open(std::string(WORK_PROJ_DIR)+"/data/lidar_path.txt",std::ios::out|std::ios::trunc);
//     gnss_file.open(std::string(WORK_PROJ_DIR)+"/data/gnss_path.txt",std::ios::out|std::ios::trunc);
//     std::cout<<"Q ： "<<q.vec().transpose()<<" t:"<<t.transpose()<<std::endl;
//     for (int i = 0;i<lidar_origin_path.size();++i)
//     {
    
//         auto lidar_trans_point = q*lidar_origin_path[i]+t;
        
//         lidar_file<<"1 0 0 "<<lidar_trans_point.x()<<" ";
//         lidar_file<<"0 1 0 "<<lidar_trans_point.y()<<" ";
//         lidar_file<<"0 0 1 "<<lidar_trans_point.z()<<std::endl;
//     }
//     for (auto &&gnss_point : gnss_path.poses)
//     {
//         gnss_file<<"1 0 0 "<<gnss_point.pose.position.x<<" ";
//         gnss_file<<"0 1 0 "<<gnss_point.pose.position.y<<" ";
//         gnss_file<<"0 0 1 "<<gnss_point.pose.position.z<<std::endl;
//     }
//     lidar_file.close();
//     gnss_file.close();
//     return true;

// }
